#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "CAStar.h"
#include "CAStar_private.h"
#include <assert.h>

int UNIT_SIZE = 3;

int TERRAIN_PASSABLE = 1;
int TERRAIN_IMPASSABLE = 0;
int TERRAIN_SOME_WHAT_PASSABLE = 2;

char TERRAIN_PASSABLE_CHAR = '_';
char TERRAIN_IMPASSABLE_CHAR = 'X';
char TERRAIN_SOME_WHAT_PASSABLE_CHAR = 'x';
char PATH_CHAR = '1';

Area area;
Path lastCalculatedPath;

void CAStar_init(int areaWidth, int areaHeight) {
	assert(areaWidth > 0);
	assert(areaHeight > 0);

	if(isInited == 1) {
		printf("[Warning !] Already init-ed!\n");
		return;
	}

	//init area
	area.width = areaWidth;
	area.height = areaHeight;
	area.terrain = malloc(areaWidth*areaHeight*sizeof(int));
	int x,y;
	for(x=0; x < areaWidth; x++) {
		for(y=0; y < areaHeight; y++) {
			area.terrain[x*areaHeight + y] = TERRAIN_PASSABLE;
		}
	}

	//init lastCalculatedPath
	lastCalculatedPath.nodesCount = 0;
	lastCalculatedPath.reachesDestination = 0;
	lastCalculatedPath.closestNode = NULL;
	lastCalculatedPath.nodes = malloc(areaWidth*areaHeight*sizeof(Node*));
	for(x=0; x < areaWidth; x++) {
		for(y=0; y < areaHeight; y++) {
			lastCalculatedPath.nodes[x*areaHeight + y] = NULL;
		}
	}

	//init nodes
	nodes.nodes = malloc(areaWidth*areaHeight*sizeof(Node*));
	for(x=0; x < areaWidth; x++) {
		for(y=0; y < areaHeight; y++) {
			nodes.nodes[x*areaHeight + y] = malloc(sizeof(Node));
			nodes.nodes[x*areaHeight + y]->x = x;
			nodes.nodes[x*areaHeight + y]->y = y;
			nodes.nodes[x*areaHeight + y]->parentsCount = 0;
			nodes.nodes[x*areaHeight + y]->parent = NULL;
			nodes.nodes[x*areaHeight + y]->g = -123;
			nodes.nodes[x*areaHeight + y]->h = -123;
			int hash = 7;
			hash = 71 * hash + nodes.nodes[x*areaHeight + y]->x;
			hash = 71 * hash + nodes.nodes[x*areaHeight + y]->y;
			nodes.nodes[x*areaHeight + y]->hashCode = hash;
		}
	}

	//init lists
	openList.size = 0;
	openList.nodePositionHash = malloc(areaWidth*areaHeight*sizeof(Node*));
	for(x=0; x < areaWidth; x++) {
		for(y=0; y < areaHeight; y++) {
			openList.nodePositionHash[x*areaHeight + y] = NULL;
		}
	}
	closedList.size = 0;
	closedList.nodePositionHash = malloc(areaWidth*areaHeight*sizeof(Node*));
	for(x=0; x < areaWidth; x++) {
		for(y=0; y < areaHeight; y++) {
			closedList.nodePositionHash[x*areaHeight + y] = NULL;
		}
	}

	//init neighbors
	neighbors.size = 0;
	int neighborIndex;
	for(neighborIndex = 0; neighborIndex<8; neighborIndex++) {
		neighbors.neighbors[neighborIndex] = NULL;
	}

	isInited = 1;
	isFreed = 0;
}

void CAStar_resetPath(int clearArea) {
	int x,y;
	//reset area
	if(clearArea) {
		for(x=0; x < area.width; x++) {
			for(y=0; y < area.height; y++) {
				area.terrain[x*area.height + y] = TERRAIN_PASSABLE;
			}
		}
	}

	//reset lastCalculatedPath
	lastCalculatedPath.nodesCount = 0;
	lastCalculatedPath.reachesDestination = 0;
	lastCalculatedPath.closestNode = NULL;

	//reset nodes
	for(x=0; x < area.width; x++) {
		for(y=0; y < area.height; y++) {
			nodes.nodes[x*area.height + y]->parentsCount = 0;
			nodes.nodes[x*area.height + y]->parent = NULL;
			nodes.nodes[x*area.height + y]->g = -123;
			nodes.nodes[x*area.height + y]->h = -123;
		}
	}

	//reset lists
	for(x=0; x < area.width; x++) {
		for(y=0; y < area.height; y++) {
			openList.nodePositionHash[x*area.height + y] = NULL;
		}
	}
	openList.size = 0;
	for(x=0; x < area.width; x++) {
		for(y=0; y < area.height; y++) {
			closedList.nodePositionHash[x*area.height + y] = NULL;
		}
	}
	closedList.size = 0;

	//reset neighbors
	neighbors.size = 0;
	int neighborIndex;
	for(neighborIndex = 0; neighborIndex<8; neighborIndex++) {
		neighbors.neighbors[neighborIndex] = NULL;
	}
}

void CAStar_calculatePath(int startX, int startY, int destinationX, int destinationY, int somewhatPassableCostMultiplicator) {
	assert(startX >= 0);
	assert(startX < area.width);
	assert(startY >= 0);
	assert(startY < area.height);
	assert(somewhatPassableCostMultiplicator >= 0);

	if(startX==destinationX && startY==destinationY) {
		Node* singleNode = nodes.nodes[startX*area.height+startY];
		lastCalculatedPath.nodes[0] = singleNode;
		lastCalculatedPath.nodesCount = 1;
		lastCalculatedPath.reachesDestination = 1;
		return;
	}

	Node n1, n2;
	n1.x = destinationX;
	n1.y = destinationY;

	n2.x = startX;
	n2.y = startY;

	double g = CAStar_private_getDistance(&n1, &n2);
	Node* finish = nodes.nodes[destinationX*area.height+destinationY];
	finish->g = g;

	Node* startNode = nodes.nodes[startX*area.height+startY];
	startNode->g = 0;
	startNode->h = CAStar_private_getDistance(&n2, finish);
	CAStar_private_List_add(&openList, startNode);

	Node* currentNode = NULL;

	int somewhatPassableGAddendum = UNIT_SIZE*somewhatPassableCostMultiplicator;

	int arrivedAtDestination = 0;
	while(openList.size > 0) {
		currentNode = CAStar_private_List_getLowestFNode(&openList);
		if(CAStar_private_Node_equals(finish, currentNode)) {
			arrivedAtDestination = 1;
			break;
		}

		CAStar_private_List_remove(&openList, currentNode->x, currentNode->y);
		CAStar_private_List_add(&closedList, currentNode);
		CAStar_private_processSucessors(currentNode, finish,somewhatPassableGAddendum);
	}

	Node* endNode = NULL;
	if(arrivedAtDestination) {
		endNode = currentNode;
	} else {
		endNode = lastCalculatedPath.closestNode;
	}

	int pathNodesCount = endNode->parentsCount+1;
	lastCalculatedPath.nodesCount = pathNodesCount;
	Node* tempNode = endNode;
	int i = 0;
	float euclidianLength = 0;
	while(tempNode!=NULL) {
		int pos = pathNodesCount-1-i;
		lastCalculatedPath.nodes[pos] = tempNode;
		euclidianLength += tempNode->g;
		i = i+1;
		tempNode = tempNode->parent;
	}

	lastCalculatedPath.reachesDestination = arrivedAtDestination;
}

void CAStar_editArea(int x, int y, int terrainType) {
	assert(x >= 0);
	assert(x < area.width);
	assert(y >= 0);
	assert(y < area.height);
	assert(terrainType==TERRAIN_IMPASSABLE || terrainType==TERRAIN_PASSABLE || terrainType==TERRAIN_SOME_WHAT_PASSABLE);

	area.terrain[x*area.height+y] = terrainType;
}

int CAStar_getTerrainAt(int x, int y) {
	assert(x >= 0);
	assert(x < area.width);
	assert(y >= 0);
	assert(y < area.height);

	if(x < 0 || x >= area.width || y < 0 || y >= area.height) {
		return TERRAIN_IMPASSABLE;
	}
	return area.terrain[x*area.height+y];
}

void CAStar_printPath() {
	char* areaString = CAStar_private_Area_toString();

	int x = 0, y = 0;
	for(y=0; y < area.height; y++) {
		for(x=0; x < area.width; x++) {
			int i;
			for(i=0; i < lastCalculatedPath.nodesCount; i++) {
				Node* pathNode = lastCalculatedPath.nodes[i];
				if(pathNode->x == x && pathNode->y == y) {
					areaString[x+(y*area.width)+y] = PATH_CHAR;
				}
			}
		}
		areaString[x+(y*area.width)+y]='\n';
	}
	printf(areaString);
	free(areaString);
}

void CAStar_free() {
	if(isFreed == 1) {
		printf("[Warning !] Already freed!\n");
		return;
	}

	int x,y;
	for(x=0; x<area.width; x++) {
		for(y=0; y<area.height; y++) {
			free(nodes.nodes[x*area.height+y]);
		}
	}
	free(nodes.nodes);

	free(area.terrain);
	free(lastCalculatedPath.nodes);
	free(openList.nodePositionHash);
	free(closedList.nodePositionHash);

	isFreed = 1;
	isInited = 0;
}






